#include "height_kf.h"


using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
	ros::init(argc, argv, "height_kf");
	ros::NodeHandle nh;
	kalman.init();
	m_kf_pub = nh.advertise<std_msgs::Float32MultiArray>("height_kf_out", 1);
	mkIMUSub = nh.subscribe("mk_imu",100,&mkIMU_Callback);
	HSubscriber = nh.subscribe ("height", 100, &HCallback);
	kbdSubscriber = nh.subscribe ("keycode", 100, &kbdCallback);
	ROS_INFO("Waiting for the start command");
	ros::spin();

}

void HCallback(const std_msgs::Float32::ConstPtr& msg)
{
	//Sonar calibration. 
	//Obtain these value from VICON.
	height=1.1337*(msg->data)-0.030912;
	
	height*=cos(mk_Imu.orientation.y)*cos(mk_Imu.orientation.x); // Project on to Z axis using roll and pitch angles.
}

void mkIMU_Callback(const sensor_msgs::ImuConstPtr& imu_msg)
{	
	if(state==RECEIVE_START)
	{
		// Running KF here since it has faster sampling rate than sonar.
		mk_Imu=*imu_msg;
		double accz = -(mk_Imu.linear_acceleration.z-g);
		estimates=kalman.process(height,accz);
		height_kf_out.data.push_back(estimates(0));
		height_kf_out.data.push_back(estimates(1));
		m_kf_pub.publish(height_kf_out);
		height_kf_out.data.clear();
	}
}

void kbdCallback (const cyphy_kbd::KeyCode& msg)
{
		char data=msg.data;
		 ROS_INFO("key_value=0x%02x\n",data);
		 switch(data)
		 {
		      case KEYCODE_SPACEBAR: state=RECEIVE_START;
		 }

}







